#include "plugin.h"
#include <stdio.h>
#include <stack>
#include <list>
#include <vector>
#include <map>
#include <algorithm>
#include <unordered_map>

double  areaOfTriangle(const Point& p0, const Point& p1, const Point& p2, double maxdist) {
	auto dx=p2.x()-p0.x();
	auto dy=p2.y()-p0.y();
	auto dz=p2.z()-p0.z();
	if(sqrt(dx*dx+dy*dy+dz*dz)>maxdist)
		return INFINITY;
	auto ux=p0.x()-p1.x();
	auto uy=p0.y()-p1.y();
	auto uz=p0.z()-p1.z();
	auto vx=p2.x()-p1.x();
	auto vy=p2.y()-p1.y();
	auto vz=p2.z()-p1.z();
	auto cx=uy*vz-uz*vy;
	auto cy=uz*vx-ux*vz;
	auto cz=ux*vy-uy*vx;
	return sqrt(cx*cx+cy*cy+cz*cz)/2;
}
std::vector<Point> reducePointsVisvalingam(const std::vector<Point>& pts, double maxdist, double maxarea) {
	// smooth
	std::list<std::pair<Point, double>> temppts;
	temppts.emplace_back(pts[0], INFINITY);
	for(size_t i=2; i<pts.size(); i++) {
		auto x=(pts[i-1].x()*2+pts[i-2].x()*1+pts[i].x()*1)/4;
		auto y=(pts[i-1].y()*2+pts[i-2].y()*1+pts[i].y()*1)/4;
		auto z=(pts[i-1].z()*2+pts[i-2].z()*1+pts[i].z()*1)/4;
		auto r=(pts[i-1].r()*2+pts[i-2].r()*1+pts[i].r()*1)/4;
		temppts.emplace_back(Point{x, y, z, r}, areaOfTriangle(pts[i-2], pts[i-1], pts[i], maxdist));
	}
	temppts.emplace_back(pts[pts.size()-1], INFINITY);
	while(temppts.size()>2) {
		double minarea=INFINITY;
		std::list<std::pair<Point, double>>::iterator miniter;
		auto i=temppts.begin();
		i++;
		auto j=i;
		j++;
		for(; j!=temppts.end(); i++, j++) {
			if(i->second<minarea) {
				minarea=i->second;
				miniter=i;
			}
		}
		if(minarea>maxarea)
			break;
		auto prev=miniter;
		prev--;
		auto next=miniter;
		next++;
		prev->first=Point{(prev->first.x()*2+miniter->first.x())/3, (prev->first.y()*2+miniter->first.y())/3, (prev->first.z()*2+miniter->first.z())/3, (prev->first.r()*2+miniter->first.r())/3};
		next->first=Point{(next->first.x()*2+miniter->first.x())/3, (next->first.y()*2+miniter->first.y())/3, (next->first.z()*2+miniter->first.z())/3, (next->first.r()*2+miniter->first.r())/3};
		temppts.erase(miniter);
		if(prev!=temppts.begin()) {
			auto pprev=prev;
			pprev--;
			prev->second=areaOfTriangle(pprev->first, prev->first, next->first, maxdist);
			if(pprev!=temppts.begin()) {
				auto ppprev=pprev;
				ppprev--;
				pprev->second=areaOfTriangle(ppprev->first, pprev->first, prev->first, maxdist);
			}
		}
		auto nnext=next;
		nnext++;
		if(nnext!=temppts.end()) {
			next->second=areaOfTriangle(prev->first, next->first, nnext->first, maxdist);
			auto nnnext=nnext;
			nnnext++;
			if(nnnext!=temppts.end()) {
				nnext->second=areaOfTriangle(next->first, nnext->first, nnnext->first, maxdist);
			}
		}
	}
	std::vector<Point> outpts{};
	for(auto& p: temppts) {
		outpts.push_back(p.first);
	}
	//for(size_t i=2; i<outpts.size(); i++)
		//fprintf(stderr, "lastarea: %lf\n", areaOfTriangle(outpts[i-2], outpts[i-1], outpts[i], maxdist));
	return outpts;
}

struct AlgFindAStar final: public AlgFind {
	static AlgFind* creater() {
		return new AlgFindAStar{};
	}
	void setParams(const std::vector<ParamValue>& params) override {
		v0=params[0].getFloat();
		length=params[1].getFloat();
		maxdist = params[2].getFloat();
		maxarea = params[3].getFloat();
	}
	std::vector<ParamValue> getParams() const override {
		std::vector<ParamValue> vl;
		vl.push_back(ParamValue{v0});
		vl.push_back(ParamValue{length});
		vl.push_back(ParamValue{ maxdist });
		vl.push_back(ParamValue{ maxarea });
		return vl;
	}
	AlgFindAStar() : AlgFind{}, length{90}, v0{.007} { }
	struct CoreNode {
		Edge e;
		size_t ei;
		//double branch;
		//size_t hit;
	};
	struct AstarNode {
		int64_t came_from;
		size_t root;
		int64_t index;
		double g_score;
		double h_score;
		double f_score;
		double meanv;
		double dirx;
		double diry;
		double dirz;
		double dist;
		double branch_dist;
		int nmean;
		bool closed;
		bool skipit;
	};
	double heuristic_cost_estimate(double dist, double meanv) {
		return 0;
		auto d=length-dist;
		if(d<0)
			return 1e12f;
		return d/(2*meanv+v0);
	}
	double dist_between(size_t ap, int64_t b, double dx, double dy, double dz, double d) {
		dx+=store[ap].dirx;
		dy+=store[ap].diry;
		dz+=store[ap].dirz;
		double r=sqrt(dx*dx+dy*dy+dz*dz)/2;
		auto aa=xfunc<0>(cube<0>(store[ap].index))*255;
		auto bb=xfunc<0>(cube<0>(b))*255;
		return (1/(aa*r+v0)+1/(bb*r+v0))*d/2;
	}
	double length;
	double v0;

	double maxdist=3.0;
	double maxarea=0.5;

	std::vector<size_t> pq;
	std::vector<CoreNode> cores;
	std::vector<AstarNode> store;
	std::map<int64_t, size_t> idx2node;
	void updateDirection(AstarNode* n, const AstarNode* np, double dx, double dy, double dz) {
		dx+=10*np->dirx;
		dy+=10*np->diry;
		dz+=10*np->dirz;
		auto len=sqrt(dx*dx+dy*dy+dz*dz);
		n->dirx=dx/len;
		n->diry=dy/len;
		n->dirz=dz/len;
	}
	void initDirection(AstarNode* n, Edge edge, size_t edgePos) {
		if(edge) {
			auto& points=edge.points();
			if(edgePos==0) {
				size_t np=17;
				if(points.size()<17) np=points.size();
				if(np>1) {
					auto& p0=points[0+np-1];
					auto& p1=points[0];
					double dx=p1.x()-p0.x();
					double dy=p1.y()-p0.y();
					double dz=p1.z()-p0.z();
					double len=sqrt(dx*dx+dy*dy+dz*dz);
					n->dirx=dx/len;
					n->diry=dy/len;
					n->dirz=dz/len;
				} else {
					n->dirx=0; n->diry=0; n->dirz=0;
				}
			} else if(edgePos==edge.length()-1) {
				size_t np=17;
				if(points.size()<17) np=points.size();
				if(np>1) {
					auto& p0=points[points.size()-1-np+1];
					auto& p1=points[points.size()-1];
					double dx=p1.x()-p0.x();
					double dy=p1.y()-p0.y();
					double dz=p1.z()-p0.z();
					double len=sqrt(dx*dx+dy*dy+dz*dz);
					n->dirx=dx/len;
					n->diry=dy/len;
					n->dirz=dz/len;
				} else {
					n->dirx=0; n->diry=0; n->dirz=0;
				}
			} else {
				n->dirx=0; n->diry=0; n->dirz=0;
			}
		} else {
			n->dirx=0; n->diry=0; n->dirz=0;
		}
	}
	size_t pushCoreNode(const Point& p, Edge e, size_t ei, size_t previ, double dx, double dy, double dz, double dist) {
		std::cerr<<"Corenode: "<<ei<<"\n";
		int64_t idx=xyzf2index<0>(p.x(), p.y(), p.z());
		store.emplace_back();
		auto& item=store[store.size()-1];
		cores.emplace_back();
		auto& core=cores[cores.size()-1];
		core.e=e;
		core.ei=ei;
		//core.branch=0;
		//core.hit=SIZE_MAX;
		item.came_from=-1;
		item.root=cores.size()-1;
		item.index=idx;
		item.nmean=1;
		item.meanv=xfunc<0>(cube<0>(idx))*255;
		item.branch_dist=0;
		item.g_score=0;
		item.h_score=heuristic_cost_estimate(item.branch_dist, item.meanv);
		if(previ==SIZE_MAX) {
			item.nmean=1;
			item.meanv=xfunc<0>(cube<0>(idx))*255;
			item.dist=0;
		} else {
			//auto& prev=store[previ];
			item.dist=dist;
		}
		item.f_score=item.g_score+item.h_score;
		item.closed=false;
		item.skipit=false;
		initDirection(&item, e, ei);
		idx2node[idx]=store.size()-1;
		return store.size()-1;
	}
	void find(const Position& p, Graph gr, Path* pth) override {
		auto AstarNodeCmp=[this](size_t a, size_t b)->bool {
			return store[a].f_score>store[b].f_score;
		};
		if(p.edge) {
			double dist_tot=0;
			for(size_t i=1; i<p.edge.points().size(); i++) {
				auto& p0=p.edge.points()[i-1];
				auto& p1=p.edge.points()[i];
				auto dist=p0.distTo(p1);
				dist_tot+=dist;
			}
			fprintf(stderr, "total dist: %lf\n", dist_tot);
		}
		///
		//double tot_branch=1;
		if(p.edge) {
			double dist_tot=0;
			auto root=pushCoreNode(p.point, p.edge, p.index, SIZE_MAX, 0, 0, 0, dist_tot);
			pq.push_back(root); std::push_heap(pq.begin(), pq.end(), AstarNodeCmp);
			auto previ=root;
			for(size_t ej=p.index+1; ej<p.edge.points().size(); ej++) {
				auto& p0=p.edge.points()[ej-1];
				auto& p1=p.edge.points()[ej];
				auto dist=p0.distTo(p1);
				dist_tot+=dist;
				//previ=pushCoreNode(p1, p.edge, ej, previ, p1.x()-p0.x(), p1.y()-p0.y(), p1.z()-p0.z(), dist_tot);
				previ=pushCoreNode(p1, p.edge, ej, SIZE_MAX, 0, 0, 0, 0);
				pq.push_back(previ); std::push_heap(pq.begin(), pq.end(), AstarNodeCmp);
				fprintf(stderr, "dist tot: %lf\n", dist_tot);
				//tot_branch+=dist;
				if(dist_tot>0)
					break;
			}
			dist_tot=0;
			previ=root;
			for(size_t ej=p.index; ej-->0; ) {
				auto& p0=p.edge.points()[ej+1];
				auto& p1=p.edge.points()[ej];
				auto dist=p0.distTo(p1);
				dist_tot+=dist;
				//previ=pushCoreNode(p1, p.edge, ej, previ, p1.x()-p0.x(), p1.y()-p0.y(), p1.z()-p0.z(), dist_tot);
				previ=pushCoreNode(p1, p.edge, ej, SIZE_MAX, 0, 0, 0, 0);
				pq.push_back(previ); std::push_heap(pq.begin(), pq.end(), AstarNodeCmp);
				fprintf(stderr, "dist tot: %lf\n", dist_tot);
				//tot_branch+=dist;
				if(dist_tot>0)
					break;
			}
		} else {
			auto root=pushCoreNode(p.point, p.edge, p.index, SIZE_MAX, 0, 0, 0, 0);
			pq.push_back(root); std::push_heap(pq.begin(), pq.end(), AstarNodeCmp);
		}

		struct { int32_t dx, dy, dz; double dist; } all_neighbors [] = {
			{-1, 0, 0, xres<0>()},
			{1, 0, 0, xres<0>()},
			{0, -1, 0, yres<0>()},
			{0, 1, 0, yres<0>()},
			{0, 0, -1, zres<0>()},
			{0, 0, 1, zres<0>()}
		};
		size_t hit=SIZE_MAX;
		//size_t next_size=1000;
		while(!pq.empty()) {
			auto currentp=pq.front();
			std::pop_heap(pq.begin(), pq.end(), AstarNodeCmp); pq.pop_back();
			if(store[currentp].skipit)
				continue;
			auto current=store[currentp].index;
			/*
			if(tot_branch/idx2node.size()<.02)
				break;
				*/
			//fprintf(stderr, "%lf\n", tot_branch/idx2node.size());
			//if(store[currentp].h_score*(store[currentp].meanv+v0)<1) {
			if(store[currentp].dist+store[currentp].branch_dist-length>-1) {
				fprintf(stderr, "d bd: %lf %lf\n", store[currentp].dist, store[currentp].branch_dist);
				if(store[currentp].branch_dist<length/3) {
					store[currentp].closed=true;
					continue;
				}
				if(store[currentp].root!=0)
					continue;
				auto par=currentp;
				while(xfunc<0>(cube<0>(store[par].index))*255<store[par].meanv*v0) {
					auto came_from=store[par].came_from;
					if(came_from<0)
						break;
					par=idx2node[came_from];
				}
				store[currentp].closed=true;
				hit=par;
				break;
			}
			/*
			if(idx2node.size()>next_size) {
				fprintf(stderr, "br/size: %lf\n", max_branch/idx2node.size());
				next_size+=1000;
			while(sync(100)) {
				usleep(1000);
			}
			}
			*/
			if(idx2node.size()>500000) {
				break;
			}
			if(!sync(100)) {
				fprintf(stderr, "abort\n");
				break;
			}
			store[currentp].closed=true;
			int32_t x, y, z;
			index2xyzi<0>(current, &x, &y, &z);
			for(auto& delta: all_neighbors) {
				int32_t neighbor_x=x+delta.dx;
				int32_t neighbor_y=y+delta.dy;
				int32_t neighbor_z=z+delta.dz;
				if(neighbor_x<0 || neighbor_x>=width<0>())
					continue;
				if(neighbor_y<0 || neighbor_y>=height<0>())
					continue;
				if(neighbor_z<0 || neighbor_z>=depth<0>())
					continue;
				int64_t neighbor=xyzi2index<0>(neighbor_x, neighbor_y, neighbor_z);
				auto neighbori=idx2node.find(neighbor);
				bool inserted=false;
				size_t nn;
				if(neighbori!=idx2node.end()) {
					auto neighborn=neighbori->second;
					if(store[neighborn].closed)
						continue;
					double tentative_g_score=store[currentp].g_score+dist_between(currentp, neighbor, delta.dx, delta.dy, delta.dz, delta.dist);
					int nmean=store[currentp].nmean+1;
					double meanv=(store[currentp].nmean*store[currentp].meanv+xfunc<0>(cube<0>(neighbor))*255)/nmean;
					double dist=store[currentp].dist+delta.dist;
					double branch_dist=store[currentp].branch_dist+delta.dist;
					double tentative_h_score=heuristic_cost_estimate(branch_dist, meanv);
					double tentative_f_score=tentative_g_score+tentative_h_score;
					//if(tentative_f_score<store[neighborn].f_score && store[neighborn].root==store[currentp].root) {
					if(tentative_f_score<store[neighborn].f_score) {
						store[neighborn].skipit=true;
						store.emplace_back();
						nn=store.size()-1;
						store[nn].g_score=tentative_g_score;
						store[nn].h_score=tentative_h_score;
						store[nn].f_score=tentative_f_score;
						store[nn].dist=dist;
						store[nn].branch_dist=branch_dist;
						store[nn].nmean=nmean;
						store[nn].meanv=meanv;
						inserted=true;
					}
				} else {
					double tentative_g_score=store[currentp].g_score+dist_between(currentp, neighbor, delta.dx, delta.dy, delta.dz, delta.dist);
					store.emplace_back();
					nn=store.size()-1;
					store[nn].nmean=store[currentp].nmean+1;
					store[nn].meanv=(xfunc<0>(cube<0>(neighbor))*255+store[currentp].meanv*store[currentp].nmean)/store[nn].nmean;
					store[nn].g_score=tentative_g_score;
					store[nn].dist=store[currentp].dist;
					store[nn].branch_dist=store[currentp].branch_dist+delta.dist;
					store[nn].h_score=heuristic_cost_estimate(store[nn].branch_dist, store[nn].meanv);
					store[nn].f_score=store[nn].g_score+store[nn].h_score;
					inserted=true;
				}
				if(inserted) {
					store[nn].came_from=current;
					store[nn].root=store[currentp].root;
					store[nn].index=neighbor;
					store[nn].closed=false;
					store[nn].skipit=false;
					//if(cores[store[currentp].root].branch<store[nn].branch_dist) {
						//tot_branch+=store[nn].branch_dist-cores[store[currentp].root].branch;
						//cores[store[currentp].root].branch=store[nn].branch_dist;
						//cores[store[currentp].root].hit=nn;
					//}
					updateDirection(&store[nn], &store[currentp], delta.dx, delta.dy, delta.dz);
					//fprintf(stderr, "N dir %f %f %f\n", store[nn].dirx, store[nn].diry, store[nn].dirz);
					idx2node[neighbor]=nn;
					pq.push_back(nn); std::push_heap(pq.begin(), pq.end(), AstarNodeCmp);
				}
			}
		}
			/*
			while(sync(1000)) {
				usleep(1000);
			}
			*/
		pth->points.clear();
		if(hit==SIZE_MAX) {
			double max_dist=-INFINITY;
			//
			for(auto i: pq) {
				auto& item=store[i];
				if(item.skipit)
					continue;
				if(item.branch_dist>max_dist) {
					max_dist=item.branch_dist;
					hit=i;
				}
			}
		}
		if(hit==SIZE_MAX) {
			pq.clear();
			store.clear();
			cores.clear();
			idx2node.clear();
			return;
		}

		std::vector<int64_t> idxes{};
		idxes.push_back(store[hit].index);
		while(store[hit].came_from>=0) {
			auto came_from=store[hit].came_from;
			hit=idx2node[came_from];
			idxes.push_back(came_from);
		}
		pth->edge0=cores[store[hit].root].e;
		pth->index0=cores[store[hit].root].ei;
		pth->edge1=Edge{};
		pth->index1=-1;
		while(idxes.size()>0) {
			auto v=idxes.back();
			idxes.pop_back();
			double mx, my, mz;
			index2xyzf<0>(v, &mx, &my, &mz);
			pth->points.push_back({mx, my, mz});
		}
		if(pth->edge0) {
			pth->points[0]=pth->edge0.points()[pth->index0];
		}

		pth->points=reducePointsVisvalingam(pth->points, maxdist, maxarea);
		pq.clear();
		store.clear();
		cores.clear();
		idx2node.clear();
		fprintf(stderr, "finished...\n");
	}
		std::vector<int32_t> getUpdate() const override {
			std::vector<int32_t> data{};
			for(auto n: pq) {
				if(store[n].skipit)
					continue;
				double x, y, z;
				index2xyzf<0>(store[n].index, &x, &y, &z);
				Point p(x, y, z);
				data.push_back(p._x);
				data.push_back(p._y);
				data.push_back(p._z);
			}
			return data;
		}
};


#include "astar.conn.cc"


static void astar_load(std::vector<AlgorithmInfo*>& algs, std::vector<GraphReaderInfo*>& readers) {
	auto algfind=new AlgInfo<AlgFind>{"AStar", 50,
		"Find paths with high intensity from a given position with A* algorithm.\nIntensity values (and transfer function) of channel 1 in group 1 is used.",
		{
			ParamSpec{"V0", "...", 0.0, 512.0, 0.7, "µm/s"},
			ParamSpec{"Length", "...", 1.0, 270.0, 90.0, "µm"},
			ParamSpec{ "MaxDist", "...", 0.0, 512.0, 3.0, "µm" },
			ParamSpec{ "MaxArea", "...", 0.0, 512.0, 0.5, "µm2" }
		},
		AlgFindAStar::creater};
	auto algconn=new AlgInfo<AlgConn>{"AStar", 50,
		"Connect points by a high intensity path with A* algorithm.\nIntensity values (and transfer function) of channel 1 in group 1 is used.",
		{
			ParamSpec{ "V0", "...", 0.0, 512.0, 0.7, "µm/s" },
			ParamSpec{ "MaxDist", "...", 0.0, 512.0, 3.0, "µm" },
			ParamSpec{"MaxArea", "...", 0.0, 512.0, 0.5, "µm2"}
		},
		AlgConnAStar::creater};
	algs.push_back(algconn);
	algs.push_back(algfind);
}

static void astar_unload() {
}

extern "C"
{
	//__declspec(dllexport) Plugin fnt_plugin { "", "AStar", "Simple built-in A* algorithms.", astar_load, astar_unload };
	PLUGIN_EXPORT 
		FNT_PLUGIN("AStar", astar_load, astar_unload, "Simple built-in A* algorithms.");
}


